/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/lqr_controller.cpp
 * @Description  : 该代码实现了一个线性二次型调节器（LQR）控制器，用于AGV的运动控制。
 *                 主要功能包括初始化LQR控制器的矩阵参数（状态矩阵A、控制矩阵B、权重矩阵Q和R），
 *                 计算A和B矩阵，更新增益矩阵，以及求解离散时间黎卡提方程。
 *                 通过LQRControl函数，根据当前状态和目标状态计算控制输入（线速度和角速度），以实现AGV的平稳移动。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:22:46
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "lqr_controller.h"
#include "tools.h"
// LQRControler::LQRControler(){
//     //"默认构造函数"
// }

/**
 * @brief LQR控制器的构造函数
 *
 * 初始化LQR控制器对象，设置状态矩阵A、控制矩阵B、权重矩阵Q和R
 *
 * @param n 状态变量的数量
 * @param dt 时间步长
 */
LQRController::LQRController(int n, double dt) : n_(n), dt_(dt) 
{
    A_.resize(3, 3);
    A_.setZero();
    B_.resize(3, 2);
    B_.setZero();
    Q_.resize(3, 3);
    Q_ << 1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 0.3;
    R_.resize(2, 2);
    R_ << 1, 0.0,
        0.0, 0.01;
}

/**
 * @brief 更新增益矩阵
 *
 * 更新线性二次调节器（LQR）的增益矩阵。
 *
 * 这个函数会根据提供的参数更新Q_和R_矩阵，并设置迭代次数和精度。
 */
void LQRController::UpdateGain() 
{
    Q_ << qx_, 0.0, 0.0,
        0.0, qy_, 0.0,
        0.0, 0.0, qtheta_;
    R_ << rv_, 0.0,
        0.0, rw_;
    n_ = iterations_;
    precision_ = eps_;   
}

/**
 * @brief 计算A和B矩阵
 *
 * 根据给定的状态点pre_point和采样时间dt_，计算LQR控制器的A和B矩阵。
 * A矩阵表示系统状态随时间的转移，B矩阵表示控制输入对系统状态的影响。
 *
 * @param pre_point 给定的状态点
 */
void LQRController::CalculateAB(State& pre_point) 
{
    A_ << 1.0, 0.0, -pre_point.speed.v * dt_ * sin(pre_point.pos.yaw),
        0.0, 1.0, pre_point.speed.v * dt_ * cos(pre_point.pos.yaw),
        0.0, 0.0, 1.0;
    B_ << dt_ * cos(pre_point.pos.yaw), 0.0,
        dt_ * sin(pre_point.pos.yaw), 0.0,
        0.0, dt_;

    std::cout << "A matrix:" << std::endl;
    for (int i = 0; i < A_.rows(); ++i) {
        for (int j = 0; j < A_.cols(); ++j) {
            std::cout << A_(i, j) << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;

    std::cout << "B matrix:" << std::endl;
    for (int i = 0; i < B_.rows(); ++i) {
        for (int j = 0; j < B_.cols(); ++j) {
            std::cout << B_(i, j) << " ";
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}


// 离散时间Riccati方程求解函数
/**
 * @brief 计算黎卡提方程（Ricatti Equation）的解
 *
 * 计算线性二次调节器（LQR）控制器的黎卡提方程的解。
 *
 * @return MatrixXd 返回黎卡提方程的解 P
 */
MatrixXd LQRController::CalculateRicatti() {
    MatrixXd P = Q_;
    for (int i = 0; i < n_; ++i) {
        MatrixXd P_next = A_.transpose() * P * A_ - A_.transpose() * P * B_ * (R_ + B_.transpose() * P * B_).inverse() * B_.transpose() * P * A_ + Q_;
        if ((P_next - P).norm() < precision_) {
            P = P_next;
            DEBUG_WARN_OUT("Cycle index is: " << i);
            break;
        }
        P = P_next;
    }
    return P;
}

/**
 * @brief 使用线性二次型调节器（LQR）控制机器人移动
 *
 * 根据机器人的当前状态和前一个点的状态，计算控制输入，使机器人能够平稳地移动到目标位置。
 *
 * @param robot_state 当前机器人的状态，包括位置（x, y）和朝向（yaw）
 * @param pre_point 前一个点的状态，包括位置（x, y）和朝向（yaw）
 *
 * @return 返回控制输入，包括线速度（v）和角速度（w）
 */
Speed LQRController::LQRControl(State& robot_state, State& pre_point) 
{
    Speed out;
    MatrixXd X(3, 1);
    X << robot_state.pos.x - pre_point.pos.x, robot_state.pos.y - pre_point.pos.y, Angle::NormAngle2(robot_state.pos.yaw - pre_point.pos.yaw);
    UpdateGain();
    CalculateAB(pre_point);
    MatrixXd P = CalculateRicatti();
    MatrixXd K = -(R_ + B_.transpose() * P * B_).inverse() * B_.transpose() * P * A_;
    MatrixXd U = K * X;
    out.v = U(0, 0);
    out.w = U(1, 0);

    // std::cout << "X mat:" << std::endl;
    // for (int i = 0; i < X.rows(); ++i) {
    //     for (int j = 0; j < X.cols(); ++j) {
    //         std::cout << X(i, j) << " ";
    //     }
    //     std::cout << std::endl;
    // }
    // std::cout << std::endl;

    // std::cout << "P mat:" << std::endl;
    // for (int i = 0; i < P.rows(); ++i) {
    //     for (int j = 0; j < P.cols(); ++j) {
    //         std::cout << P(i, j) << " ";
    //     }
    //     std::cout << std::endl;
    // }
    // std::cout << std::endl;
    // std::cout << "K mat:" << std::endl;
    // for (int i = 0; i < K.rows(); ++i) {
    //     for (int j = 0; j < K.cols(); ++j) {
    //         std::cout << K(i, j) << " ";
    //     }
    //     std::cout << std::endl;
    // }
    // std::cout << std::endl;

    // ROS_INFO_STREAM( "X mat:" << std::endl);
    // for (int i = 0; i < X.rows(); ++i) {
    //     for (int j = 0; j < X.cols(); ++j) {
    //         ROS_INFO_STREAM(  X(i, j) << " ");
    //     }
    //     ROS_INFO_STREAM(  std::endl);
    // }
    // ROS_INFO_STREAM(std::endl);

    // ROS_INFO_STREAM( "P mat:" << std::endl);
    // for (int i = 0; i < P.rows(); ++i) {
    //     for (int j = 0; j < P.cols(); ++j) {
    //         ROS_INFO_STREAM( P(i, j) << " ");
    //     }
    //     ROS_INFO_STREAM( std::endl);
    // }
    // ROS_INFO_STREAM( std::endl);
    // ROS_INFO_STREAM( "K mat:" << std::endl);
    // for (int i = 0; i < K.rows(); ++i) {
    //     for (int j = 0; j < K.cols(); ++j) {
    //         ROS_INFO_STREAM( K(i, j) << " ");
    //     }
    //     ROS_INFO_STREAM( std::endl);
    // }
    // ROS_INFO_STREAM( std::endl);
    // ROS_INFO_STREAM( "U mat:" << std::endl);
    // for (int i = 0; i < U.rows(); ++i) {
    //     for (int j = 0; j < U.cols(); ++j) {
    //         ROS_INFO_STREAM( U(i, j) << " ");
    //     }
    //     ROS_INFO_STREAM( std::endl);
    // }
    // ROS_INFO_STREAM( std::endl);


    return out;
}

